# -*- coding: utf-8 -*-
"""
Created on Mon Jan  3 18:01:05 2022

@author: wulong
"""
import numpy as np

def Weight_MPC(input_cont_bnds_lo_r, input_cont_bnds_up_r, state_bnds_lo_r, state_bnds_up_r):
    
    delta_y1 = 1
    delta_y2 = 0.1
    u1_u2 = 0.8*(input_cont_bnds_up_r[0] + input_cont_bnds_up_r[1])
    u1 = 0.8*input_cont_bnds_up_r[0]
    
    delta_state = 0.08*(state_bnds_up_r - state_bnds_lo_r)
    delta_input = 0.08*(input_cont_bnds_up_r - input_cont_bnds_lo_r)
    
    
    weight_y1 = 1/delta_y1**2
    weight_y2 = 1/delta_y2**2
    weight_u1_u2 = 1/u1_u2
    weight_u1 = 1/u1
    
    
    weight_state = np.ones(23)/delta_state**2
    
    weight_state_s = np.append(np.insert(weight_state[18:21], 0, 
                                         weight_state[16]), weight_state[22])
    weight_state_m = np.append(weight_state[3:9], 
                               np.array([weight_state[11], weight_state[14], 
                                         weight_state[21]]))
    weight_state_f = np.delete(weight_state, np.array((3,4,5,6,7,8,11,14,16,18,19,20,21,22)))
    
    
    weight_input = np.ones(6)/delta_input**2
    
    weight_input_s = np.insert(weight_input[4:], 0, weight_input[2])
    weight_input_m = weight_input[1]
    weight_input_f = np.array([weight_input[0], weight_input[3]])
    
    weight = (weight_y1, weight_y2, weight_u1_u2, weight_u1, 
              weight_state_s, weight_state_m, weight_state_f, 
              weight_input_s, weight_input_m, weight_input_f)
    
    return weight


